#pragma once

#include "_float.h"
#include "_int.h"
#define _USE_MATH_DEFINES
#include <cmath>
#include <map>
#include <vector>

namespace Algorithm {

	template<typename pixel>
	struct Bitmap {
		struct KeyPoint{
			unsigned x;
			unsigned y;
			float magnitude;
			int orientation;
			std::vector<float> fingerprint;
		};
		struct Feature{
			int x;
			int y;
			float f;
		};
	private:
		std::vector<Feature> Features;
		std::vector<KeyPoint> KeyPoints;
		pixel* RawData;
		unsigned Width;
		unsigned Height;
		std::map<float, Bitmap<float>*> blurredMagnitudeMtxs;

	public:
		void addFeature(int x, int y, float f)
		{
			Features.push_back(Feature());
			Feature& fea = Features.back();
			fea.x = x;
			fea.y = y;
			fea.f = f;
		}
		const std::vector<KeyPoint>& keyPoints()
		{
			return KeyPoints;
		}
		const std::vector<Feature>& features()
		{
			return Features;
		}

		Bitmap(unsigned width, unsigned height) :
			Width(width),
			Height(height)
		{
			RawData = new pixel[Width*Height];
		}

		Bitmap(Bitmap& bitmap, unsigned left, unsigned top, unsigned width, unsigned height) :
			Height(height),
			Width(width)
		{
			int max_w = bitmap.getWidth()-1;
			int max_h = bitmap.getHeight()-1;
			RawData = new pixel[width*height];
			for(int i=0; i<width; ++i)
			{
				for(int j=0; j<height; ++j)
				{
					RawData[j*width + i] =
						bitmap.RawData[
							std::max(std::min((int)top+j, (int)max_h), 0) * width +
								std::max(std::min((int)left+i, (int)max_w), 0)];
				}
			}
		}

		~Bitmap()
		{
			delete[] RawData;
			if (!blurredMagnitudeMtxs.empty())
			{
				for (auto it = blurredMagnitudeMtxs.begin(); it != blurredMagnitudeMtxs.end(); it++)
				{
					delete it->second;
				}
			}
		}

		pixel& getPixel(unsigned x, unsigned y)
		{
			return RawData[y*Width + x];
		}

		const pixel& getPixel(unsigned x, unsigned y) const
		{
			return RawData[y*Width + x];
		}

		pixel getInterpolatedPixel(int x, int y) const
		{
			if (x > 0 && x < (int)Width -1 && y > 0 && y < (int)Height - 1)
			{
				return (getPixel(x,y)+getPixel(x+1,y)+getPixel(x,y+1)+getPixel(x+1,y+1))/4;
			}
			else
			{
				return pixel(0);
			}
		}

		unsigned getHeight()
		{
			return Height;
		}

		unsigned getWidth()
		{
			return Width;
		}

		float getMagnitude(int x, int y) const
		{
			int x1 = std::max(std::min((int)Width, x+1), 0);
			int x1n = std::max(std::min((int)Width, x-1), 0);
			int y1 = std::max(std::min((int)Height, y+1), 0);
			int y1n = std::max(std::min((int)Height, y-1), 0);
			return sqrt(pow(getPixel(x1,y)-getPixel(x1n,y),2) + pow(getPixel(x,y1)-getPixel(x,y1n),2));
		}

		float getBlurredMagnitude(unsigned x, unsigned y, float sigma)
		{
			if (blurredMagnitudeMtxs[sigma] == 0)
			{
				blurredMagnitudeMtxs[sigma] = createBlurredMagnitudeMtx(sigma);
			}
			return blurredMagnitudeMtxs[sigma]->getPixel(x,y);
		}

		Bitmap<float>* createBlurredMagnitudeMtx(float sigma)
		{
			Bitmap<float>* magnitudeMtx = new Bitmap(Width,Height);
			for (int i = 0; i < (int)Width; i++)
			{
				for (int j = 0; j < (int)Height; j++)
				{
					magnitudeMtx->getPixel(i,j) = getMagnitude(i,j);
				}
			}
			Filter<float> gauss = Filter<float>::createGauss(sigma, 1.5f);
			Bitmap<float>* gauss_O_magnitudeMtx = gauss.Run(*magnitudeMtx);
			delete magnitudeMtx;
			return gauss_O_magnitudeMtx;
		}

		float getOrientation(int x, int y) const
		{
			int x1 = std::max(std::min((int)Width, x+1), 0);
			int x1n = std::max(std::min((int)Width, x-1), 0);
			int y1 = std::max(std::min((int)Height, y+1), 0);
			int y1n = std::max(std::min((int)Height, y-1), 0);
			float a = getPixel(x,y1);
			float b = getPixel(x,y1n);
			float c = getPixel(x1,y);
			float d = getPixel(x1n,y);
			if (c != d)
			{
				return atan2((a-b),(c-d));
			}
			else
			{
				return 0;
			}
		}

		std::vector<float> getOrientationHistogram(unsigned x, unsigned y, float sigma)
		{
			std::vector<float> histogram(40, 0.0f);
			float maxr = 1.5f * sigma;
			int minx = (int)(x - maxr);
			int maxx = (int)(x + maxr);
			int miny = (int)(y - maxr);
			int maxy = (int)(y + maxr);
			minx = std::max(std::min((int)Width, minx), 0);
			maxx = std::max(std::min((int)Width, maxx), 0);
			miny = std::max(std::min((int)Height, miny), 0);
			maxy = std::max(std::min((int)Height, maxy), 0);
			for (int i = minx; i <= maxx; i++)
			{
				for (int j = miny; j <= maxy; j++)
				{
					float magnitude = getBlurredMagnitude(i,j, sigma);
					int orientationInDegrees = (int)(ceil(getOrientation(i,j)*180/M_PI));
					if (orientationInDegrees < 0)
						orientationInDegrees += 360;
					histogram[orientationInDegrees/9] += magnitude;
				}
			}
			return histogram;
		}

		void addKeyPoint(unsigned x, unsigned y, float sigma)
		{
			std::vector<float> histogram = getOrientationHistogram(x, y, sigma);
			float magnitude = getBlurredMagnitude(x,y,sigma);
			float max = 0;
			int orientation = 0;
			for (int i = 0; i < 40; i++)
			{
				if (histogram[i] > max)
				{
					max = histogram[i];
					orientation = i;
				}
			}
			KeyPoint keyPoint;
			keyPoint.x = x;
			keyPoint.y = y;
			keyPoint.magnitude = magnitude;
			keyPoint.orientation = orientation;
			std::vector<float> fingerprint = calculateFingerprint(x,y);
			keyPoint.fingerprint = rotateFingerprint(fingerprint, orientation/5);
			KeyPoints.push_back(keyPoint);
			for (int i = 0; i < 40; i++)
			{
				if (i != orientation && histogram[i] >= 0.8 * max)
				{
					keyPoint.orientation = i;
					keyPoint.fingerprint = rotateFingerprint(fingerprint, i/5);
					KeyPoints.push_back(keyPoint);
				}
			}
		}

		std::vector<float> calculateFingerprint(int x, int y)
		{
			std::vector<float> result(128, 0.0f);
			std::vector<float> histogram(8, 0.0f);
			float squareSum = 0;
			for (int i = 0; i < 4; i++)
			{
				for (int j = 0; j < 4; j++)
				{
					for (int k = 0; k < 4; k++)
					{
						for (int l = 0; l < 4; l++)
						{
							pixel center = getInterpolatedPixel(x+(i-2)*4+k,y+(j-2)*4+l);
							pixel left = getInterpolatedPixel(x+(i-2)*4+k-1,y+(j-2)*4+l);
							pixel right = getInterpolatedPixel(x+(i-2)*4+k+1,y+(j-2)*4+l);
							pixel top = getInterpolatedPixel(x+(i-2)*4+k,y+(j-2)*4+l-1);
							pixel bottom = getInterpolatedPixel(x+(i-2)*4+k,y+(j-2)*4+l+1);
							float magnitude = sqrt(pow(right-left,2)+pow(bottom-top,2));
							int orientation;
							if (bottom != top && left != right)
							{
								orientation = (int)ceil(atan2((bottom-top), (right-left))*180/M_PI+180)%360;
								if (orientation < 0)
									orientation = 360 + orientation;
							}
							else
							{
								magnitude = 0;
								orientation = 0;
							}
							float sqrR = (float)(pow((i-2)*4+k,2)+pow((j-2)*4+l,2));
							double sqrSigma_2 = 2.0*std::pow(6, 2.0);
							float gaussWeight = (float)(std::exp(-sqrR/sqrSigma_2)/sqrSigma_2);
							histogram[orientation/45] += gaussWeight * magnitude;
						}
					}
					for (int k = 0; k < 8; k++)
					{
						result[i*8+k+j*4*8] = histogram[k];
						squareSum += histogram[k] * histogram[k];
					}
				}
			}
			squareSum = sqrt(squareSum);
			for (int i = 0; i < 128; i++)
			{
				result[i] /= squareSum;
			}
			return result;
		}

		std::vector<float> rotateFingerprint(const std::vector<float>& fingerprint, int orientation)
		{
			std::vector<float> rotatedFingerprint(128, 0.0f);
			for (int i = 0; i < 16; i++)
			{
				for (int j = 0; j < 8; j++)
				{
					int newIndex = (j>=orientation) ? j-orientation : 8-(orientation-j);
					rotatedFingerprint[i*8+newIndex] = fingerprint[i*8+j];
				}
			}
			return rotatedFingerprint;
		}
	};

	struct Index
	{
		int X;
		int Y;

		Index(int x, int y) :
			X(x),
			Y(y)
		{ }
	};

	struct IndexCmp
	{
		bool operator()(const Index& one, const Index& two)
		{
			return one.X < two.X || (one.X == one.X && one.Y < two.Y);
		}
	};

	template<typename pixel>
	class Filter {

		std::map<Index, float, IndexCmp> Items;
		double sumWeight;

		int _minx;
		int _maxx;
		int _miny;
		int _maxy;

		bool _separated;

	public:
		int getMinX() {
			return _minx;
		}

		int getMaxX() {
			return _maxx;
		}

		int getMinY() {
			return _miny;
		}

		int getMaxY() {
			return _maxy;
		}

		bool getSeparated() {
			return _separated;
		}

		pixel calculatePixel(Bitmap<pixel>& src, unsigned pixelX, unsigned pixelY) const
		{
			if(!_separated)
			{
				pixel sum = pixel(0.0f);
				int max_width = src.getWidth()-1;
				int max_height = src.getHeight()-1;
				for(const std::pair<Index, float>& pair : Items)
				{
					int x = pixelX + pair.first.X;
					int y = pixelY + pair.first.Y;

					// clamp
					x = std::max(std::min(max_width, x), 0);
					y = std::max(std::min(max_height, y), 0);
				
					sum = sum + (src.getPixel(x, y))*pair.second;	// we use just the R channel
				}
				return sum/(float)sumWeight;
			}
			else
			{
				pixel sum = 0;
				int max_width = src.getWidth()-1;
				for(const std::pair<Index, float>& pair : Items)
				{
					int x = pixelX + pair.first.X;

					// clamp
					x = std::max(std::min(max_width, x), 0);
				
					sum = sum + (calculatePixelSeparatedTransponse(src, x, pixelY))*pair.second;	// we use just the R channel
				}
				return sum/(float)sumWeight;
			}
		}

		pixel calculatePixelSeparated(Bitmap<pixel>& src, unsigned pixelX, unsigned pixelY) const
		{
			pixel sum = 0;
			int max_width = src.getWidth()-1;
			for(const std::pair<Index, float>& pair : Items)
			{
				int x = pixelX + pair.first.X;

				// clamp
				x = std::max(std::min(max_width, x), 0);
				
				sum = sum + (src.getPixel(x, pixelY))*pair.second;	// we use just the R channel
			}
			return sum/(float)sumWeight;
		}

		pixel calculatePixelSeparatedTransponse(Bitmap<pixel>& src, unsigned pixelX, unsigned pixelY) const
		{
			pixel sum = 0.0f;
			int max_height = src.getHeight()-1;
			for(const std::pair<Index, float>& pair : Items)
			{
				int y = pixelY + pair.first.X;

				// clamp
				y = std::max(std::min(max_height, y), 0);
				
				sum = sum + (src.getPixel(pixelX, y))*pair.second;	// we use just the R channel
			}
			return sum/(float)sumWeight;
		}

		std::string ToString() const {
			std::string ret;
			for(const std::pair<Index, float>& pair : Items) {
				ret += "(" + pair.first.X + ", " + pair.first.Y + ") : " + pair.first.second + "\n";
			}
			ret += "sum: " + sumWeight + "\n";
			ret += "sep: " + _separated;
			return ret;
		}

		Bitmap<pixel>* Run(Bitmap<pixel>& src) {
			Bitmap<pixel>* ret = new Bitmap<pixel>(src.getWidth(), src.getHeight());
			if(!_separated) {
				for(unsigned i=0; i<src.getWidth(); ++i) {
					for(unsigned j=0; j<src.getHeight(); ++j) {
						ret->getPixel(i, j) = calculatePixel(src, i, j);
					}
				}
			}
			else {
				Bitmap<pixel>* dst = new Bitmap<pixel>(src.getWidth(), src.getHeight());
				for(unsigned i=0; i<dst->getWidth(); ++i) {
					for(unsigned j=0; j<dst->getHeight(); ++j) {
						dst->getPixel(i, j) = calculatePixelSeparated(src, i, j);
					}
				}
				for(unsigned i=0; i<ret->getWidth(); ++i) {
					for(unsigned j=0; j<ret->getHeight(); ++j) {
						ret->getPixel(i, j) = calculatePixelSeparatedTransponse(*dst, i, j);
					}
				}
			}
			return ret;
		}

		static Filter createGauss(double sigma) {
		/*	Not separated
			Filter^ ret = gcnew Filter();
			double sqrSigma_2 = 2.0*System::Math::Pow(sigma, 2.0);
			double maxr = 2.5f*sigma;
			ret->_minx = ret->_miny = -(int)maxr;
			ret->_maxx = ret->_maxy = (int)maxr;
			ret->sumWeight = 0.0;
			for(int i=ret->_minx; i<=ret->_maxx; ++i) {
				for(int j=ret->_miny; j<=ret->_maxy; ++j) {
					double sqrR = (double)(i*i + j*j);
					double r = System::Math::Sqrt(sqrR);
					if(r <= maxr)
					{
						double weight = System::Math::Exp(-sqrR/sqrSigma_2)/sqrSigma_2; // No need for /PI
						ret->Items.Add(gcnew Index(i, j), weight);
						ret->sumWeight += weight;
					}
				}
			}
			return ret;*/

			return createGauss(sigma, 2.5f);
		}

		static Filter createGauss(double sigma, float range)
		{
			Filter ret;
			ret._separated = true;
			double sqrSigma_2 = 2.0*std::pow(sigma, 2.0);
			double maxr = range*sigma;
			ret._miny = ret._maxy = 0;
			ret._minx = -(int)maxr;
			ret._maxx = (int)maxr;
			ret.sumWeight = 0.0;
			for(int i=ret._minx; i<=ret._maxx; ++i) {
				double sqrR = (double)(i*i);
				double r = i;
				if(r <= maxr)
				{
					double weight = std::exp(-sqrR/sqrSigma_2)/sqrSigma_2; // No need for /PI
					ret.Items[Index(i, 0)] = (float)weight;
					ret.sumWeight += weight;
				}
			}
			return ret;
		}
	};

	void Harris(Bitmap<float>& src, float diffSigma, float intSigma, float lambda,  bool use_log=false);
	Bitmap<float>* Hesse(Bitmap<float>& src, float diffSigma);
}